#include "ZhangPoseEstimator.hpp"

using namespace cv;
using namespace std;

// Matrix notations
// - CvMat is Main
// - Mat is minor
// - 'inp' trail is input data

//#define DISPLAY

// supplementary routines
void ZhangPoseEstimator::CvMatShow(const CvMat* mat)   // added const works for both cases i.e. CvMat* & const CvMat*
{
	/* create missed variable matrix */
	//uchar cell;  // 1st method
	float *data = mat->data.fl;  // 2nd method
	int n = mat->cols;

	printf("mat size (row, col) = (%d,%d)\n",mat->rows,mat->cols);

	for( int i = 0; i < mat->rows; i++ ){
		for( int j = 0; j < mat->cols; j++ )
		{
			//cell = (int)(CV_MAT_ELEM(*mat,uchar,i,j) );  // 1st method
			//printf(" %d",cell;
			printf(" %0.2f",data[i*n+j]);  // 2nd method
		}
		printf("\n");
	}
}

void ZhangPoseEstimator::Estimate(const Mat objectPointsMat, const Mat imagePointsMat,
								  Mat& rvecMat, Mat& tvecMat)
{

#ifdef DISPLAY
	// call solvePnP
	//Mat rvecMat, tvecMat;
	cout << "object points:" << endl;
	MATmtxdisp(objectPointsMat);
	cout << "image points:" << endl;
	MATmtxdisp(imagePointsMat);
	cout << "cam matrix:" << endl;
	MATmtxdisp(CamMat);
	cout << "distortion coefficients:" << endl;
	MATmtxdisp(distCoeffsMat);
#endif

	solvePnP(objectPointsMat, imagePointsMat, CamMat, distCoeffsMat, rvecMat, tvecMat, useExtrinsicGuess);

	// return results
	Mat rotmat;
	Rodrigues(rvecMat, rotmat);
	rotmat.copyTo(rvecMat);

	//MATmtxdisp(rvecMat);
	//MATmtxdisp(tvecMat);
}

ZhangPoseEstimator::ZhangPoseEstimator(const cv::Mat CamMatInp, const cv::Mat distCoeffsMatInp,
									   bool useExtrinsicGuessInp)
{
	//CamMat = CamMatInp;
	CamMatInp.copyTo(CamMat);
	//distCoeffsMat = distCoeffsMatInp;
	distCoeffsMatInp.copyTo(distCoeffsMat);
	useExtrinsicGuess = useExtrinsicGuessInp;
}

ZhangPoseEstimator::ZhangPoseEstimator()
{

	// init parameters
	float camA[9] = {762, 0, 320, 0, 761, 240, 0 , 0 , 1};    // calibrated already
	//double camA[9] = {762, 0, 320, 0, 761, 240, 0 , 0 , 1};    // calibrated already
	CvMat Amat = cvMat( 3, 3, CV_32F, camA);   // cam matrix
	CvMat* A = &Amat;   // aware of passing CvMat addr to pointer
	CamMat = Mat(&Amat, true);  // BUG HERE!! -> solved: 'false' -> 'true' since local vars (e.g. Amat, A) are temporal

	float distCoeffsmat_ar[5] = {0, 0, 0, 0, 0};
	//double distCoeffsmat_ar[5] = {0, 0, 0, 0, 0};
	CvMat distCoeffsmat = cvMat( 1, 5, CV_32F, distCoeffsmat_ar);
	CvMat* distCoeffs = &distCoeffsmat; 
	distCoeffsMat = Mat(&distCoeffsmat, true);

	useExtrinsicGuess = false;
}


ZhangPoseEstimator::ZhangPoseEstimator(const string camMatFile, const string distCoeffFile)
{

	// read camMat
	FileStorage fs(camMatFile.c_str(), FileStorage::READ);	
	fs["camera_matrix"] >> CamMat;

	FileStorage fs1(distCoeffFile.c_str(), FileStorage::READ);	
	fs1["distortion_coefficients"] >> distCoeffsMat;

	MATmtxdisp(CamMat);
	MATmtxdisp(distCoeffsMat);

	useExtrinsicGuess = false;
}


ZhangPoseEstimator::~ZhangPoseEstimator()
{

}


int ZhangPoseEstimator::getCamParams(Mat& ret_CamMat, Mat& ret_Intrins)
{
	int ret = 1; 

	ret_CamMat = CamMat;
	ret_Intrins = distCoeffsMat;

	return ret;
}